/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>

#include "ccamtransform.h"
#include "../error/cerror.h"

using namespace std;
using namespace cv;

CCamTransform::CCamTransform()
{
    history_length = 5;
}

CCamTransform::CCamTransform(const string& file)
{
    history_length = 5;
    LoadIntrinsicParams(file);
}


void CCamTransform::Reserve() 
{
    history_length = 5;
}

CCamTransform::~CCamTransform() 
{

}


/*!
    \fn CCamTransform::CalibrateIntrisictParam(IplImage * img, CvSize pattern_size, double h)
 */
bool CCamTransform::CalibrateIntrisictParams(CImageSource& src, Size psize, double h, bool calc_distortion_tang) 
{
    int  board_found, nviews;
    Mat img;

    cout << "Введите колличество сцен:" << endl;
    cin  >> nviews;
    //nviews = 2;

    vector<vector<Point2f> > img_points;
    vector<vector<Point3f> > obj_points;
    vector<Mat> rvecs;
    vector<Mat> tvecs;

    namedWindow("Calibration", CV_WINDOW_AUTOSIZE);

    for (int k = 0; k < nviews; k++) {
        std::cout << "Захват сцены " << k  << std::endl;

        img_points.push_back(vector<Point2f>());
        obj_points.push_back(vector<Point3f>());
        do 
	{
            img = Mat(src.GrabFrame());

            board_found = findChessboardCorners(Mat(img), psize, img_points.back());

            if (board_found) 
	    {		
		cout << "Калибровочная структура опознана." << endl;
	    }
            else 
	    {
		cout << "Калибровочная структура НЕ опознана." << endl;
	    }

            if (board_found) 
	    {
		drawChessboardCorners(img, psize, Mat(img_points.back()), board_found);
	    }
            imshow("Calibration", img);

            cout << "Нажмите ENTER." << endl;
            waitKey(0);
        } while (!board_found);

        for (int i = 0; i < img_points.back().size(); i++) 
	{
            obj_points.back().push_back(Point3f(h*(i % psize.width), h*(i / psize.width), 0.0));
            //cout << img_points.back()[i] << " -> " << obj_points.back()[i] << endl;
        }
    }

    cvDestroyWindow("Calibration");
    calibrateCamera(obj_points, img_points, img.size(), intrinsic, distortion, rvecs, tvecs);

    for (vector<Mat>::iterator j = rvecs.begin(), k = tvecs.begin(); j != rvecs.end(); j++,k++)
    {
      Mat_<double> tmp = *j; 
      cout << "rvecs: (" << tmp(0,0) << " " << tmp(0,1) << " " << tmp(0,2) << ")"  << endl;     
      tmp = *k;
      cout << "tvecs: (" << tmp(0,0) << " " << tmp(0,1) << " " << tmp(0,2) << ")"  << endl;     
    } 
    
    rot_vect = rvecs.front();
    trans_vect = tvecs.front();
    Rodrigues(rot_vect, rot_mat);

    cout << "Калибровка завершена." << endl;

    return board_found;
}


/*!
    \fn CCamTransform::SaveIntrinsicParams(const char * file)
 */
void CCamTransform::SaveIntrinsicParams(const string& file) const 
{
    FileStorage fs(file, FileStorage::WRITE);
    if (!fs.isOpened()) 
    {
	throw Errors::CError("CCamTransform: Unable open file " + file);
    }
    fs << "intrinsic" << intrinsic << "distortion" << distortion;
}


/*!
    \fn CCamTransform::LoadIntrinsicParam(const char * file)
 */
void CCamTransform::LoadIntrinsicParams(const string& file)
{
    FileStorage fs(file, FileStorage::READ);
    if (!fs.isOpened()) 
    {
	throw Errors::CError("CCamTransform: Unable open file: " + file);
    }
    fs["intrinsic"] >> intrinsic;
    fs["distortion"] >> distortion;
}

/*!
    \fn CCamTransform::SaveTransform( SetDistortionModeconst char * file)
 */
void CCamTransform::SaveTransform(const string& file) const 
{
    FileStorage fs(file, FileStorage::WRITE);
    if(!fs.isOpened())
    {
	throw Errors::CError("CCamTransform: Couldn't create/open file: " + file);
    }	
    fs << "rot_vect" << rot_vect << "rot_mat" << rot_mat << "trans_vect" << trans_vect;
}


/*!
    \fn CCamTransform::LoadTransform(const char * file)
 */
void CCamTransform::LoadTransform(const string& file)
{
    FileStorage fs(file, FileStorage::READ);
    if (!fs.isOpened()) 
    {
	throw Errors::CError("CCamTransform: Unable open file: " + file);
    }

    fs["rot_vect"] >> rot_vect;
    fs["rot_mat"] >> rot_mat;
    fs["trans_vect"] >> trans_vect;
}

/*!
    \fn CCamTransform::CalcTransform()
 */
void CCamTransform::CalcTransform() 
{
    if (i_points.size() < 4) 
    {
	throw Errors::CBadArgError();
    }

    solvePnP(Mat(o_points), Mat(i_points), intrinsic, distortion, rot_vect, trans_vect, false);
    Rodrigues(rot_vect, rot_mat);
}

bool CCamTransform::RecalcTransform() 
{
    if (i_points.size() < 4) 
    {
	return false;	
    }

    solvePnP(Mat(o_points), Mat(i_points), intrinsic, distortion, rot_vect, trans_vect, true);
    Rodrigues(rot_vect, rot_mat);
}

void CCamTransform::RemoveDistortion(const Mat& src, Mat& dst) const 
{
    undistort(src, dst, intrinsic, distortion);
}

/*!
    \fn CCamTransform::ImgToObj(const Point3f& obj)
 */
Point2f CCamTransform::ProjectPoint(const Point3f& obj) const 
{
    vector<Point3f> o_point;
    vector<Point2f> i_point;

    o_point.push_back(obj);
    projectPoints(Mat(o_point), rot_vect, trans_vect, intrinsic, distortion, i_point);

    return i_point.front();
}

Point3f CCamTransform::ReverseProjection(const Point2f& pt, const HPlane3f& plane) const 
{
    double p;

    Mat_<Vec2f> img_pt(1, 1), cam_pt(1, 1);

    img_pt(0,0) = Vec2f(pt.x, pt.y);
    undistortPoints(img_pt, cam_pt, intrinsic, distortion);
    Point2f pt_undistoted = Point2f(cam_pt(0,0)[0], cam_pt(0,0)[1]);

    // 3 Reverse transform
    //plane in camera coordinates
    Mat_<double> tmp(3,1);
    HPlane3f cam_plane = ToCameraCoord(plane);
    
    //calculate position in camera coordinates
    p = - cam_plane.D / (cam_plane.A * pt_undistoted.x + cam_plane.B * pt_undistoted.y + cam_plane.C);
    tmp(0, 0) = p * pt_undistoted.x;
    tmp(1, 0) = p * pt_undistoted.y;
    tmp(2, 0) = p;
    //transform to object coordinates
    tmp -= trans_vect.t();
    tmp = rot_mat.t()*tmp;

    return Point3f(tmp(0,0), tmp(1,0), tmp(2,0));
}

void CCamTransform::PrintIParams() const 
{
    cout << "fx = " << intrinsic(0, 0) << endl;
    cout << "fy = " << intrinsic(1, 1) << endl;
    cout << "cx = " << intrinsic(0, 2) << endl;
    cout << "cy = " << intrinsic(1, 2) << endl;
    cout << "dist = ";
    for (int i = 0; i < 4; i++) cout << " " << distortion(0, i);
    cout << endl;
}

void CCamTransform::PrintTParams() const 
{
    cout << "t = (";
    for (int i = 0; i < 3; i++) cout << " " << trans_vect(0, i);
    cout << " )*" << endl;

    cout << "rot_vect = (";
    for (int i = 0; i < 3; i++) cout << " " << rot_vect(0, i);
    cout << " )*" << endl;

    cout << "rot_mat = (" << endl;;
    for (int j = 0; j < 3; j++) 
    {
        for (int i = 0; i < 3; i++)  cout << " " << rot_mat(j, i);
        cout << endl;
    }
    cout << " )" << endl;
    
    cout << "angle of sys. cordinates rotation: " << sqrt(rot_vect.dot(rot_vect)) << endl;
    Mat_<double> tmp = -rot_mat*trans_vect.t();
    cout << "reference point of camera in global sys. coord.: " << Point3f(tmp(0,0), tmp(1,0), tmp(2,0)) << endl;
}

HPlane3f CCamTransform::ToCameraCoord(const HPlane3f& plane) const 
{
    //plane in camera coordinates
    Mat_<double> tmp1(3,1);
    Mat_<double> tmp2(3,1);
    tmp1(0,0) = plane.A;
    tmp1(1,0) = plane.B;
    tmp1(2,0) = plane.C;
    tmp2 = rot_mat*tmp1;
    return HPlane3f(tmp2(0,0), tmp2(1,0), tmp2(2,0), plane.D - tmp2.dot(trans_vect.t()));
 }
  
Mat_<double>	CCamTransform::getIntrinsicMat() const
{
    return intrinsic;
}

Mat_<double>	CCamTransform::getReverseIntrinsicMat() const
{
    Mat_<double> rev_intr(Mat_<double>::zeros(3,3));
       
    rev_intr(0,0) = 1/intrinsic(0,0);
    rev_intr(1,1) = 1/intrinsic(1,1);
    rev_intr(0,2) = -intrinsic(0,2)/intrinsic(0,0);
    rev_intr(1,2) = -intrinsic(1,2)/intrinsic(1,1);    
    rev_intr(2,2) = 1;
    
    return rev_intr;
}

Mat	CCamTransform::getRemovePerspectiveMat() const
{
    Point2f src_p[4] = {Point2f(0, 0), 
			Point2f(100, 0), 
			Point2f(0, 100), 
			Point2f(100, 100)};
    src_p[0] = ProjectPoint(Point3f(src_p[0].x, src_p[0].y, 0));    
    src_p[1] = ProjectPoint(Point3f(src_p[1].x, src_p[1].y, 0));
    src_p[2] = ProjectPoint(Point3f(src_p[2].x, src_p[2].y, 0));
    src_p[3] = ProjectPoint(Point3f(src_p[3].x, src_p[3].y, 0));

    double 	len1 = max(abs(src_p[1].x - src_p[0].x), abs(src_p[1].y - src_p[0].y)),
		len2 = max(abs(src_p[0].y - src_p[2].y), abs(src_p[0].x - src_p[2].x)); 
    
    Point2f dst_p[4] = {Point2f(319, 239),			// center 
			Point2f(319 + len1 , 239), 		// from c. to positive(right) x
			Point2f(319, 239 + len2), 		// from c. to positive(below) y
			Point2f(319 + len1, 239 + len2)};	// diagonal, positive x and y
    return getPerspectiveTransform(src_p, dst_p);
}

void CCamTransform::SetHistoryLength(int len) 
{
    if (len <= 3) throw Errors::CBadArgError();
    history_length = len;
}

void CCamTransform::AddPair(const Point2f& img, const Point3f& obj) 
{
    i_points.push_back(img);
    o_points.push_back(obj);
}

void CCamTransform::DelPair() 
{
    i_points.erase(i_points.begin());
    o_points.erase(o_points.begin());
}

void CCamTransform::NewPair(const Point2f& img, const Point3f& obj) 
{
    while (i_points.size() > history_length) 
    {
        i_points.erase(i_points.begin());
        o_points.erase(o_points.begin());
    }
    AddPair(img, obj);
}

void CCamTransform::ClearPairs() 
{
    i_points.clear();
    o_points.clear();
}

